/*


*/


boolean leftObstacleSensor();
boolean rightObstacleSensor();


//
// the loop routine runs over and over again forever:
// 


void loop()
{
  
  // These are Variables - they store values and you
  // can use IF to see what value they hold - read on!
  int cm, lcm, rcm;
  
  // ROBOTEERS Add you code here!!!!! 
  // Remember 1000ms means 1 second 500ms means 1/2 second
  // Speed values are between 0 and 255
  
 
  readSensors();  // Best to keep this here
  
  
  // You can use this to print where you are in the loop
  //Serial.println("I am in the loop");
  
   // This sketch will look for empty space 
   // could you make it follow you ?

  // Robot moves forward a bit then stops
  forward(100, 200);
  halt(50);
  
  // Now we get the distance in the variable cm
  cm = Ultrasonic();

  // if the distance is LESS than 30 cm then execeute the code
  if(cm < 30)

  {
    halt(0);

    pointLeft();
    
    lcm = Ultrasonic();
    
    pointRight();
    
    rcm = Ultrasonic();
    
    // Point the sensor 
    pointCentre();
    
    reverse(400, 255);
    
    halt(0);
    
    // Clever bit here - we have looked right and left - which way to go!
    if (rcm < lcm) {
      rightSpin(200,255);
      halt(0);
    }
      
    else
    {
      leftSpin(200,255);
      halt(0);
    }
    
    halt(0);

  }
  
  // Repeat!
    
}




// Dont add code below here


// robMove routine switches the correct inputs to the L298N for the direction selected.
void robMove(int l1, int l2, int r1, int r2)
{
  analogWrite(L1, l1);
  digitalWrite(L2, l2);
  analogWrite(L3, r1);
  digitalWrite(L4, r2);  
}

void reverse(int wait, int vSpeed)
{
  Serial.println("Moving backward: " + String(vSpeed));
  robMove(255-vSpeed, HIGH, 255-vSpeed, HIGH);    // when reversing, the speed needs to be opposite, so subtract from 255
  delay(wait);
}

void forward(int wait, int vSpeed)
{
  Serial.println("Moving forward: " + String(vSpeed));
  robMove(vSpeed, LOW, vSpeed, LOW);
  delay(wait);
}

void rightSpin(int wait, int vSpeed)
{
  Serial.println("Spinning right");
  robMove(vSpeed, LOW, 0, HIGH);
  delay(wait);
  robMove(0, LOW, 0, LOW);
}

void leftSpin(int wait,int vSpeed)
{
  Serial.println("Spinning left");
  robMove(0, HIGH, vSpeed, LOW);
  delay(wait);
  robMove(0, LOW, 0, LOW);
}

void rightTurnFd(int wait, int lSpeed, int rSpeed)
{
  Serial.println("Turning right forward");
  robMove(lSpeed, LOW, rSpeed, LOW);
  delay(wait);
}

void leftTurnFd(int wait, int lSpeed, int rSpeed)
{
  Serial.println("Turning left forward");
  robMove(lSpeed, LOW, rSpeed, LOW);
  delay(wait);
}

void rightTurnBd(int wait, int lSpeed, int rSpeed)
{
  Serial.println("Turning right backward");
  robMove(255-lSpeed, HIGH, 255-rSpeed, HIGH);
  delay(wait);
}

void leftTurnBd(int wait, int lSpeed, int rSpeed)
{
  Serial.println("Turning left backward");
  robMove(255-lSpeed, HIGH, 255-rSpeed, HIGH);
  delay(wait);
}

void halt(int wait)
{
  Serial.println("Stopping");
  robMove(0, LOW, 0, LOW);
  delay(wait);
}

// Sensors Functions

void readSensors()
{
  leftObstacleSensor();
  rightObstacleSensor();
  Ultrasonic();
  
  
  Left  = analogRead(A4);           // Read value from left sensor
  Right = analogRead(A5);           // Read value from right sensor
  
  if ( Left < Ref )
    Left = BLACK;
  if ( Left > Ref )
    Left = WHITE;
    
  if ( Right < Ref )
    Right = BLACK;
  if ( Right > Ref )
    Right = WHITE;
  
}

boolean leftObstacleSensor()
{
 
  if (digitalRead(lDetect)==0) // Test left sensor
  {
    Serial.println("Left Detected");
    leftSensor=true;
    return true;
  }
  leftSensor=false;
  return false;
}

boolean rightObstacleSensor()
{
 
  if (digitalRead(rDetect)==0) // Test left sensor
  {
    Serial.println("Right Detected");
    leftSensor=true;
    return true;
  }
  leftSensor=false;
  return false;

}


int Ultrasonic()
{
 
   delay(50);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  
  unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  //Serial.print("Ping: ");
  
  
  cm=uS / US_ROUNDTRIP_CM;
  
  if ( cm == 0 ) 
    cm = 1000;  // Clear!

  //Serial.print(cm);
  //Serial.println("cm");
  
  return cm;
}


void pointLeft()
{
  panServo.write(servoMax);

  Serial.println("Point Left");

  delay(150); // wait for servo to get there
}

void pointRight()
{
    Serial.println("Point Right");
  panServo.write(servoMin);
  delay(300); // wait for servo to get there
}

void pointCentre()
{
  Serial.println("Point Centre");
  panServo.write(panCentre);
  delay(150); // wait for servo to get there
}

void pointValue(int pos)
{
  Serial.print("Point Value: ");
  Serial.println(pos);
  panServo.write(pos);
  delay(150); // wait for servo to get there
}


void tiltUp()
{
  Serial.println("Tilt Up");
  tiltServo.write(servoMin);
  delay(150); // wait for servo to get there
}

void tiltDown()
{
  Serial.println("Tilt Down");
  tiltServo.write(servoMax);
  delay(300); // wait for servo to get there
}

void tiltCentre()
{
  Serial.println("Tilt Centre");
  tiltServo.write(panCentre);
  delay(150); // wait for servo to get there
}

void tiltCentre(int pos)
{
  Serial.print("Tilt Value: ");
  Serial.println(pos);
  tiltServo.write(pos);
  delay(150); // wait for servo to get there
}

void testAll()
{
long startTime;  // Captures starttime in clock time - time is milieocns between them
long endTime;
  
  // ROBOTEERS this is the test all funcion - it will execute this once soit needs to be called from loop
  // 
  
  // Can change the value here for speed - 0 -> 255
  //  parameter is delay in ms after performing operation
  
  readSensors();  // Best to keep this here
  
  // Basic code - just remove this or add commenrts 
  
  Serial.println("TESTING MOVEMENT SHOULD TAKE ABOUT 10 seconds");
  Serial.println("FULL POWER 255");
  forward(1000,255);
  halt(1000);
  
  reverse(1000,255);
  halt(1000);
  
  leftSpin(1000,255);
  halt(1000);
  
  rightSpin(1000,255);
  halt(1000);
 
  Serial.println("HALF POWER 180");
  forward(1000,180);
  halt(1000);
  
  reverse(1000,180);
  halt(1000);
  
  leftSpin(1000,180);
  halt(1000);
  
  rightSpin(1000,180);
  halt(1000);
 
  
 Serial.println("NOW TESTING PAN AND TILT 5 TIMES");
 Serial.println("TESTING PAN 5 TIMES - PLEASE ENSURE THIS IS A PAN ACTION");
 
  for ( int i=0;i<5;i++)
  {
    pointCentre();
    delay(500);
    pointLeft();
    delay(500);
    pointRight();
    delay(500);
    pointCentre();
    
    delay(1000);
  }
  
 Serial.println("TESTING TITL 5 TIMES - PLEASE ENSURE THIS IS A TILT ACTION");
 
  for ( int i=0;i<5;i++)
  {
    tiltCentre();
    delay(500);
    tiltUp();
    delay(500);
    tiltDown();
    delay(500);
    tiltCentre();
    
    delay(1000);
  }
 
  
  Serial.println("NOW TESTING OBSTACLE SENSORS FOR 30 SECONDS ");
  Serial.println("TEST SENSOR NOW PLEASE - TILT and PAN SENSOR WILL MOVE");

  startTime = millis();
  endTime = millis();
  
  while(endTime-startTime < 30000)
  {
    
    if ( leftObstacleSensor() == true )
    {
       pointLeft();
       tiltUp();
    }
  
    if ( rightObstacleSensor() == true )
    {
       pointRight();
       tiltDown();
    }
    
    endTime = millis();
    readSensors();  // Best to keep this here
  }
  
  Serial.println("NOW TESTING ULTRASONIC SENSORS FOR 30 SECONDS ");
  Serial.println("TEST SENSOR BY PLACING YOUR HAND OR OBJECT IN FRONT OF ULTRASONIC ( EYES)");
  Serial.println("IF THE DISTANCE IS LESS THAN 90CM IT SHOULD PRINT");

  startTime = millis();
  endTime = millis();
  
  while(endTime-startTime < 30000)
  {

  
    if ( Ultrasonic() < 90 ) {
      Serial.print("Distance is: ");
      Serial.println(Ultrasonic());
    }
    
    endTime = millis();
    readSensors();  // Best to keep this here
  }
  
    pointCentre();
    tiltCentre();
  
  Serial.println("NOW TESTING ULTRASONIC SENSORS FOR 30 SECONDS ");
  Serial.println("TEST SENSOR NOW PLEASE USING BLACK or WHITE for EACH SENSOR");

  startTime = millis();
  endTime = millis();
  
  while(endTime-startTime < 30000)
  {
    
    if ( Left == BLACK ) 
      Serial.println("LEFT SENSOR READS BLACK");
   
   
    if ( Left == WHITE )    
     Serial.println("LEFT SENSOR READS WHITE");
    
    if ( Right == BLACK ) 
      Serial.println("RIGHT SENSOR READS BLACK");
   
   
    if ( Right == WHITE )    
     Serial.println("RIGHT SENSOR READS WHITE");
    
    
    
    
    endTime = millis();
    readSensors();  // Best to keep this here
  }
  
  
    
}




